/*
mpc_v2s 必须与 mpc_v2m 配合使用
*/

# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "dataStruct.h"
# include "tracking_base.h"
# include "UartCommunicate.h"

class mpc_v2s : public tracking_base
{
private:
    std::unique_ptr<cpp_tools::UartVectorPort<double>> m_uart;

    cpp_tools::mtDataManager<bool> busy; // 一个🔓，用于防止在响应前多次向v2m发送指令或请求，类型没有任何意义，只是因为不能给void所以随便给了一个

    int timeOut_ms = 10;
    double ping = -1;

    std::string UART_device = "/tmp/V0";
    int UART_bauds = 921600;

    double lastSteering = 0;
public:
    mpc_v2s()
        : tracking_base()
    {
        cpp_tools::XmlParameterManager m_xml("device", host_path + settings_path + "/device/settings.xml");
        bool writeXML = false;
        if (!m_xml.getParameter("mpc_v2s", "fileName", UART_device))
        {
            std::cout << "mpc_v2s设备路径使用使用默认值: " << UART_device << std::endl;
            m_xml.setParameter("mpc_v2s", "fileName", UART_device);

            writeXML = true;
        }
        if (!m_xml.getParameter("mpc_v2s", "bauds", UART_bauds))
        {
            std::cout << "mpc_v2s波特率使用使用默认值: " << UART_bauds << std::endl;
            m_xml.setParameter("mpc_v2s", "bauds", UART_bauds);

            writeXML = true;
        }
        if (writeXML)
        {
            m_xml.saveToFile(host_path + settings_path + "/device/settings.xml");
        }

        m_uart = std::make_unique<cpp_tools::UartVectorPort<double>>(UART_device, UART_bauds);
        m_uart.get()->clearBuffer();
    };

    ~mpc_v2s()
    {
        std::cout << "mpc controller exit" << std::endl;
    };

    /// @brief 读取状态估计的数据并进行转化
    void update(Eigen::VectorXd const & state, double delta, double F_x, double time) override
    {
        auto ac = busy.write();

        Eigen::VectorX<double> order(11);
        order << static_cast<double>(data::communicate::lower_solver::mpc_v2_order::update), state;

        m_uart.get()->sendVector(order);
    }

    std::pair<double,double> getControl(Eigen::VectorXd const & state, double delta, double F_x, double time) override
    {
        auto ac = busy.write();

        Eigen::VectorX<double> message(1);
        message << static_cast<double>(data::communicate::lower_solver::mpc_v2_order::getControl);
        
        m_uart.get()->clearBuffer();
        m_uart.get()->sendVector(message);
        
        cpp_tools::clock m_clock;
        if (m_uart.get()->receiveVector(message, timeOut_ms) > 0)
        {
            if (message.size() >= 3 && message(0) == static_cast<double>(data::communicate::lower_solver::mpc_v2_order::getControl))
            {
                ping = m_clock.toc(0);

                lastSteering = message(1);

                return {message(1), message(2)};
            }
        }

        return {lastSteering, 0};
    }

    double getComputTime() override
    {
        return ping;
    }

    int refresh() override
    {
        auto ac = busy.write();

        Eigen::VectorX<double> message(1);
        message << static_cast<double>(data::communicate::lower_solver::mpc_v2_order::refresh);

        m_uart.get()->sendVector(message);

        ping = -1;

        if (m_uart.get()->receiveVector(message, timeOut_ms) > 0)
        {
            std::cout << "刷新完成" << std::endl;
        }
        else
        {
            std::cout << "刷新超时" << std::endl;
        }

        return 0;
    }

    mpc_v2s(const mpc_v2s&) = delete;
};
